#include <iostream>
#include <fstream>

#include "camera_cross_view_base_opencv_gpu.h"

#define DEVICE 0
using namespace flann;
static Logger gLogger;
samplesCommon::Args gArgs;
template <typename T>
using SampleUniquePtr = std::unique_ptr<T, samplesCommon::InferDeleter>;
unique_ptr<GPUAllocator> gpu_mem(new GPUAllocator);
typedef unsigned char uchar;

void Camera_cross_view_base_opencv_gpu::Init()
{
    // load config json
    std::ifstream in(config_file_path_);
    in >> camera_cross_view_base_config_json;
    if (camera_cross_view_base_config_json.is_null())
    {
        std::cout << "camera_cross_view_base_config_json is null" << std::endl;
        return;
    }

    // config task config
    input_concate_image_mask = camera_cross_view_base_config_json["task_config"]["input_concate_image_mask"];
    input_concate_image_mask_car_name = camera_cross_view_base_config_json["task_config"]["input_concate_image_mask_car_name"];
    model_type = camera_cross_view_base_config_json["task_config"]["model_type"];
    delete_sky = camera_cross_view_base_config_json["task_config"]["delete_sky"];
    delete_sky_proportion = camera_cross_view_base_config_json["task_config"]["delete_sky_proportion"];
    post_processing = camera_cross_view_base_config_json["task_config"]["post_processing"];
    erode_dilate = camera_cross_view_base_config_json["task_config"]["erode_dilate"];
    show_result = camera_cross_view_base_config_json["task_config"]["show_result"];

    // load trt model information
    trtfile = camera_cross_view_base_config_json["model_config"]["trtfile_name"];
    trtfile = config_file_path_.substr(0, (config_file_path_.find_last_of("/") + 1)) + trtfile;
    concate_image_count = camera_cross_view_base_config_json["model_config"]["concate_image_count"];

    // load post processing
    if (post_processing && erode_dilate)
    {
        erode_element = getStructuringElement(erode_shape,
                                              Size(camera_cross_view_base_config_json["post_processing_config"]["erode_dilate"]["erode_k_size"],
                                                   camera_cross_view_base_config_json["post_processing_config"]["erode_dilate"]["erode_k_size"]));
        dilate_element = getStructuringElement(dilate_shape,
                                               Size(camera_cross_view_base_config_json["post_processing_config"]["erode_dilate"]["dilate_k_size"],
                                                    camera_cross_view_base_config_json["post_processing_config"]["erode_dilate"]["dilate_k_size"]));
        erode_iterations = camera_cross_view_base_config_json["post_processing_config"]["erode_dilate"]["erode_iterations"];
        dilate_iterations = camera_cross_view_base_config_json["post_processing_config"]["erode_dilate"]["dilate_iterations"];
        erode_anchor = cv::Point(camera_cross_view_base_config_json["post_processing_config"]["erode_dilate"]["erode_anchor"],
                                 camera_cross_view_base_config_json["post_processing_config"]["erode_dilate"]["erode_anchor"]);
        dilate_anchor = cv::Point(camera_cross_view_base_config_json["post_processing_config"]["erode_dilate"]["dilate_anchor"],
                                  camera_cross_view_base_config_json["post_processing_config"]["erode_dilate"]["dilate_anchor"]);
    }

    // load image information
    show_image_resize_height = camera_cross_view_base_config_json["image_config"]["show_image_resize_height"];
    show_image_resize_width = camera_cross_view_base_config_json["image_config"]["show_image_resize_width"];
    for (size_t i = 0; i < 3; i++)
    {
        mean[i] = camera_cross_view_base_config_json["image_config"]["mean"][i];
    }
    for (size_t i = 0; i < 3; i++)
    {
        std[i] = camera_cross_view_base_config_json["image_config"]["std"][i];
    }
    infer_distance_front = camera_cross_view_base_config_json["image_config"]["infer_distance_front"];
    infer_distance_back = camera_cross_view_base_config_json["image_config"]["infer_distance_back"];
    infer_distance_left = camera_cross_view_base_config_json["image_config"]["infer_distance_left"];
    infer_distance_right = camera_cross_view_base_config_json["image_config"]["infer_distance_right"];
    input_mask_scale = camera_cross_view_base_config_json["image_config"]["input_mask_scale"];

    // load input image mask point

    // init engine
    if (false == Cross_view_model_init(trtfile))
    {
        std::cout << "model init fail" << std::endl;
        return;
    }

    // init  ros
    if (false == Init_Ros())
    {
        std::cout << "ros init fail" << std::endl;
        return;
    }
}

bool Camera_cross_view_base_opencv_gpu::Cross_view_model_init(const std::string &trtfile)
{
    // load model information
    net_input_height_ = camera_cross_view_base_config_json["model_config"]["net_input_height"];
    net_input_width_ = camera_cross_view_base_config_json["model_config"]["net_input_width"];
    net_input_channel_ = camera_cross_view_base_config_json["model_config"]["net_input_channel"];
    net_input_batch_size_ = camera_cross_view_base_config_json["model_config"]["net_input_batch_size"];
    net_output_height_ = camera_cross_view_base_config_json["model_config"]["net_output_height"];
    net_output_width_ = camera_cross_view_base_config_json["model_config"]["net_output_height"];
    net_output_channel_ = camera_cross_view_base_config_json["model_config"]["net_output_channel"];
    net_output_batch_size_ = net_input_batch_size_;

    fstream file;
    IHostMemory *trtModelStream{nullptr};
    IPluginFactory *onnxPligin;

    // load trt file
    std::cout << "Load trt file from: " << trtfile << std::endl;
    file.open(trtfile, ios::binary | ios::in);
    file.seekg(0, ios::end);
    int length = file.tellg();
    file.seekg(0, ios::beg);
    std::unique_ptr<char[]> data(new char[length]);
    file.read(data.get(), length);
    file.close();
    std::cout << "Load trt file done!" << std::endl;

    // trt file deserialize
    std::cout << "Deserializing" << std::endl;
    auto trtRuntime = SampleUniquePtr<nvinfer1::IRuntime>(nvinfer1::createInferRuntime(gLogger.getTRTLogger()));
    engine = trtRuntime->deserializeCudaEngine(data.get(), length, onnxPligin);
    context = engine->createExecutionContext();
    cout << "Deserialize done!" << endl;

    // check model
    std::vector<int64_t> bufferSize;
    int nb_bindings = engine->getNbBindings();
    vector<string> binding_names(nb_bindings);
    for (int i = 0; i < nb_bindings; ++i)
    {
        auto dim = context->getEngine().getBindingDimensions(i);
        string name = context->getEngine().getBindingName(i);
        size_t size = dim.d[0] * dim.d[1] * dim.d[2] * dim.d[3];
        cout << "i=" << i << " tensor's name:"
             << name
             << " dim:("
             << dim.d[0] << ","
             << dim.d[1] << ","
             << dim.d[2] << ","
             << dim.d[3] << ")"
             << " size:" << size
             << endl;
        binding_names[i] = name;
        engine_name_size.emplace(name, make_pair(i, size));
    }
    net_output_tensor_size = net_output_height_ * net_output_width_ * net_output_channel_;
    net_input_image_size = net_input_height_ * net_input_width_ * net_input_channel_;
    CHECK_CUDA(cudaStreamCreate(&stream));
    CHECK_CUDA(cudaMalloc(&buffers[0], net_input_image_size * sizeof(float)));
    CHECK_CUDA(cudaMalloc(&buffers[1], net_output_tensor_size * sizeof(float)));

    return true;
}

bool Camera_cross_view_base_opencv_gpu::Init_Ros()
{
    // base set
    last_frame_timestamp = 0;
    advertise_queue_size = camera_cross_view_base_config_json["ros_config"]["message"]["adv_q_size"];
    subscribe_queue_size = camera_cross_view_base_config_json["ros_config"]["message"]["sub_q_size"];

    // input topic and message
    for (size_t i = 0; i < concate_image_count; i++)
    {
        image_raw_input_topic_vect.push_back(
            camera_cross_view_base_config_json["ros_config"]["ros_topic"]["image_raw_input_topic"][i]);
        sub_cam_message_ptr_vect.push_back(new message_filters::Subscriber<sensor_msgs::Image>(
            ros_nodehandle,
            image_raw_input_topic_vect[i],
            1));
    }

    // setup output topic and message
    pub_cross_view_result_image = it_.advertise(
        camera_cross_view_base_config_json["ros_config"]["ros_topic"]["cross_view_result_image_topic"],
        advertise_queue_size);
    if (show_result)
    {
        pub_cross_view_result_show_image = it_.advertise(
            camera_cross_view_base_config_json["ros_config"]["ros_topic"]["cross_view_result_show_image_topic"],
            advertise_queue_size);
        pub_cross_view_input_net_image = it_.advertise(
            camera_cross_view_base_config_json["ros_config"]["ros_topic"]["cross_view_input_net_image_topic"],
            advertise_queue_size);
    }

    std::cout << "Model input batch size: " << net_input_batch_size_ << std::endl;
    std::cout << "Concate image count: " << concate_image_count << std::endl;
    if (1 == concate_image_count)
    {
        // road_information_contours = ros_road_information_contours.advertise<ros_interface::RegionContoursClassesImages>(
        //     camera_cross_view_base_config_json["ros_config"]["ros_topic"]["road_information_contours_topic"],
        //     advertise_queue_size);
        sub = ros_nodehandle.subscribe<sensor_msgs::Image>(
            image_raw_input_topic_vect[0],
            1,
            &Camera_cross_view_base_opencv_gpu::Callback_Cross_view_base,
            this);
    }
    else if (3 == concate_image_count)
    {
        // road_information_contours = ros_road_information_contours.advertise<ros_interface::RegionContoursClassesImages>(
        //     camera_cross_view_base_config_json["ros_config"]["ros_topic"]["road_information_contours_topic"],
        //     advertise_queue_size);
        message_filters::Synchronizer<MySyncPolicy_2> *sync;
        sync = new message_filters::Synchronizer<MySyncPolicy_2>(MySyncPolicy_2(subscribe_queue_size),
                                                                 *(sub_cam_message_ptr_vect[2]),
                                                                 *(sub_cam_message_ptr_vect[1]),
                                                                 *(sub_cam_message_ptr_vect[0]));
        sync->registerCallback(boost::bind(
            &Camera_cross_view_base_opencv_gpu::Callback_Cross_view_base,
            this, _1, _2, _3));
    }
    else if (4 == concate_image_count)
    {
        // road_information_contours = ros_road_information_contours.advertise<ros_interface::RegionContoursClassesImages>(
        //     camera_cross_view_base_config_json["ros_config"]["ros_topic"]["road_information_contours_topic"],
        //     advertise_queue_size);
        message_filters::Synchronizer<MySyncPolicy_3> *sync;
        sync = new message_filters::Synchronizer<MySyncPolicy_3>(MySyncPolicy_3(subscribe_queue_size),
                                                                 *(sub_cam_message_ptr_vect[0]),
                                                                 *(sub_cam_message_ptr_vect[1]),
                                                                 *(sub_cam_message_ptr_vect[2]),
                                                                 *(sub_cam_message_ptr_vect[3]));
        sync->registerCallback(boost::bind(
            &Camera_cross_view_base_opencv_gpu::Callback_Cross_view_base,
            this, _1, _2, _3, _4));
    }
    else if (6 == concate_image_count)
    {
        // road_information_contours = ros_road_information_contours.advertise<ros_interface::RegionContoursClassesImages>(
        //     camera_cross_view_base_config_json["ros_config"]["ros_topic"]["road_information_contours_topic"],
        //     advertise_queue_size);
        message_filters::Synchronizer<MySyncPolicy_5> *sync;
        sync = new message_filters::Synchronizer<MySyncPolicy_5>(MySyncPolicy_5(subscribe_queue_size),
                                                                 *sub_cam_message_ptr_vect[5],
                                                                 *sub_cam_message_ptr_vect[1],
                                                                 *sub_cam_message_ptr_vect[2],
                                                                 *sub_cam_message_ptr_vect[3],
                                                                 *sub_cam_message_ptr_vect[4],
                                                                 *sub_cam_message_ptr_vect[0]);
        sync->registerCallback(boost::bind(
            &Camera_cross_view_base_opencv_gpu::Callback_Cross_view_base,
            this, _1, _2, _3, _4, _5, _6));
    }

    ROS_INFO("Init end.\n");
    std::cout << "Starting inference:" << std::endl;

    return true;
}

void Camera_cross_view_base_opencv_gpu::Callback_Cross_view_base(const sensor_msgs::ImageConstPtr &image_message_0)
{
    std::vector<sensor_msgs::ImageConstPtr> image_message_vect;
    image_message_vect.push_back(image_message_0);

    Camera_cross_view_base_opencv_gpu::Cross_view_inference_function(image_message_vect);

    return;
}

void Camera_cross_view_base_opencv_gpu::Callback_Cross_view_base(const sensor_msgs::ImageConstPtr &image_message_0,
                                                                 const sensor_msgs::ImageConstPtr &image_message_1,
                                                                 const sensor_msgs::ImageConstPtr &image_message_2)
{
    std::vector<sensor_msgs::ImageConstPtr> image_message_vect;
    image_message_vect.push_back(image_message_2);
    image_message_vect.push_back(image_message_1);
    image_message_vect.push_back(image_message_0);

    Camera_cross_view_base_opencv_gpu::Cross_view_inference_function(image_message_vect);

    return;
}

void Camera_cross_view_base_opencv_gpu::Callback_Cross_view_base(const sensor_msgs::ImageConstPtr &image_message_0,
                                                                 const sensor_msgs::ImageConstPtr &image_message_1,
                                                                 const sensor_msgs::ImageConstPtr &image_message_2,
                                                                 const sensor_msgs::ImageConstPtr &image_message_3)
{
    std::vector<sensor_msgs::ImageConstPtr> image_message_vect;
    image_message_vect.push_back(image_message_0);
    image_message_vect.push_back(image_message_1);
    image_message_vect.push_back(image_message_2);
    image_message_vect.push_back(image_message_3);

    Camera_cross_view_base_opencv_gpu::Cross_view_inference_function(image_message_vect);

    return;
}

void Camera_cross_view_base_opencv_gpu::Callback_Cross_view_base(const sensor_msgs::ImageConstPtr &image_message_0,
                                                                 const sensor_msgs::ImageConstPtr &image_message_1,
                                                                 const sensor_msgs::ImageConstPtr &image_message_2,
                                                                 const sensor_msgs::ImageConstPtr &image_message_3,
                                                                 const sensor_msgs::ImageConstPtr &image_message_4,
                                                                 const sensor_msgs::ImageConstPtr &image_message_5)
{
    std::vector<sensor_msgs::ImageConstPtr> image_message_vect;
    image_message_vect.push_back(image_message_5);
    image_message_vect.push_back(image_message_1);
    image_message_vect.push_back(image_message_2);
    image_message_vect.push_back(image_message_3);
    image_message_vect.push_back(image_message_4);
    image_message_vect.push_back(image_message_0);

    Camera_cross_view_base_opencv_gpu::Cross_view_inference_function(image_message_vect);

    return;
}

void Camera_cross_view_base_opencv_gpu::Cross_view_inference_function(std::vector<sensor_msgs::ImageConstPtr> image_message_vect)
{
    startTime_0 = clock(); //计时开始

    std::vector<cv_bridge::CvImageConstPtr> cv_ptr_vect_share;
    float *prediction;

    // image preprocess
    total_prepare_startTime_0 = clock();
    cv_bridge_toCvCopy_startTime_0 = clock();

    try
    {
        for (auto image_message : image_message_vect)
        {
            cv_ptr_vect_share.push_back(cv_bridge::toCvShare(image_message, sensor_msgs::image_encodings::BGR8));
        }
    }
    catch (cv::Exception &e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }
    std::vector<cv::Mat> image_vect;
    for (size_t i = 0; i < image_message_vect.size(); i++)
    {
        image_vect.push_back(cv_ptr_vect_share[i]->image);
    }

    cv_bridge_toCvCopy_endTime_0 = clock();
    std::cout << "Cv_bridge toCvCopy time is:" << (double)(cv_bridge_toCvCopy_endTime_0 - cv_bridge_toCvCopy_startTime_0) / CLOCKS_PER_SEC << "s" << std::endl;

    PrepareImage_concate(image_vect, (float *)buffers[0]);


    total_prepare_endTime_0 = clock();
    std::cout << "Total prepare image time is:" << (double)(total_prepare_endTime_0 - total_prepare_startTime_0) / CLOCKS_PER_SEC << "s" << std::endl;

    // start inference
    model_infer_startTime_0 = clock();

    CHECK_CUDA(cudaMallocHost((void **)&prediction, sizeof(float) * (net_output_tensor_size)));
    context->enqueue(net_input_batch_size_, buffers, stream, nullptr);
    CHECK_CUDA(cudaMemcpyAsync(prediction, buffers[1], sizeof(float) * net_output_tensor_size, cudaMemcpyDeviceToHost, stream));
    cudaStreamSynchronize(stream);

    cv::Mat model_inference_result(net_output_height_, net_output_width_,
                                   CV_8UC1, cv::Scalar(0));
    float *temp_prediction = new float[net_output_tensor_size];
    memcpy(temp_prediction, prediction, net_output_tensor_size * sizeof(float));

    // for (int i = 0; i < 10 ; i++)
    // {
    //     std::cout << temp_prediction[i] << std::endl;
    //     std::cout << temp_prediction[i+512*512] << std::endl;
    // }
    // std::cout << temp_prediction[0] << std::endl;

    model_infer_endTime_0 = clock();
    std::cout << "Model infer time is:" << (double)(model_infer_endTime_0 - model_infer_startTime_0) / CLOCKS_PER_SEC << "s" << std::endl;

    // post processing
    post_processing_startTime_0 = clock();

    cv::Mat model_inference_result_0(net_output_height_, net_output_width_,
                                     CV_32FC1, cv::Scalar(0));
    cv::Mat model_inference_result_1(net_output_height_, net_output_width_,
                                     CV_32FC1, cv::Scalar(0));
    for (int k = 0; k < net_output_height_; k++)
    {
        for (int j = 0; j < net_output_width_; j++)
        {
            model_inference_result_0.at<float>(k, j) = temp_prediction[j + k * net_output_width_];
        }
    }

    for (int k = 0; k < net_output_height_; k++)
    {
        for (int j = 0; j < net_output_width_; j++)
        {
            model_inference_result_1.at<float>(k, j) = temp_prediction[j + k * net_output_width_ + net_output_tensor_size / 2];
        }
    }
    for (int k = 0; k < net_output_height_; k++)
    {
        for (int j = 0; j < net_output_width_; j++)
        {
            if (model_inference_result_1.at<float>(k, j) > model_inference_result_0.at<float>(k, j))
            {
                model_inference_result.at<uchar>(k, j) = 255;
            }
        }
    }



    // cv::imshow("debug",model_inference_result);
    // cv::waitKey(0);

    delete temp_prediction;
    CHECK_CUDA(cudaFreeHost(prediction));

    post_processing_endTime_0 = clock();
    std::cout << "Post processing time is:" << (double)(post_processing_endTime_0 - post_processing_startTime_0) / CLOCKS_PER_SEC << "s" << std::endl;

    // erode and dilate
    erode_and_dilate_startTime_0 = clock();

    if (post_processing && erode_dilate)
    {
        cv::Mat model_inference_result_class_dilate_dst(net_output_height_, net_output_width_,
                                                        CV_8UC1, cv::Scalar(0));
        if (1 <= erode_iterations)
        {
            cv::erode(model_inference_result,
                      model_inference_result_class_dilate_dst,
                      erode_element,
                      erode_anchor,
                      erode_iterations);
        }
        else
        {
            model_inference_result_class_dilate_dst = model_inference_result;
        }
        if (1 <= dilate_iterations)
        {
            cv::dilate(model_inference_result_class_dilate_dst,
                       model_inference_result,
                       dilate_element,
                       dilate_anchor,
                       dilate_iterations);
        }
        else
        {
            model_inference_result = model_inference_result_class_dilate_dst;
        }
    }

    erode_and_dilate_endTime_0 = clock();
    std::cout << "Erode and dilate time is:" << (double)(erode_and_dilate_endTime_0 - erode_and_dilate_startTime_0) / CLOCKS_PER_SEC << "s" << std::endl;

    // 旋转图片
    // cv::Point2f model_inference_result_center;
    // cv::Mat rot_mat(2, 3, CV_32FC1);
    // cv::Mat model_inference_result_rotation(net_output_height_, net_output_width_,
    //                                         CV_8UC1, cv::Scalar(0));

    // model_inference_result_center.x = int(model_inference_result.cols / 2);
    // model_inference_result_center.y = int(model_inference_result.rows / 2);
    // rot_mat = cv::getRotationMatrix2D(model_inference_result_center, 90, 1);
    // cv::warpAffine(model_inference_result,
    //                model_inference_result_rotation,
    //                rot_mat,
    //                model_inference_result_rotation.size());

    // 正方向
    // cv::line(model_inference_result_rotation,
    //          cv::Point(0, 0), cv::Point(model_inference_result_rotation.cols, 0), 255, 5);

    // temp_cv_image.image = model_inference_result_rotation;

    // show result image
    if (show_result)
    {
        cv_bridge::CvImage temp_cv_show_image;
        sensor_msgs::Image temp_ros_show_image;
        cv::Mat model_inference_result_show_resize(net_output_height_, net_output_width_,
                                                   CV_8UC1, cv::Scalar(0));
        cv::resize(model_inference_result, model_inference_result_show_resize, cv::Size(show_image_resize_width, show_image_resize_height));

        // 中心点
        cv::Point model_inference_result_rotation_center;
        if (1 == concate_image_count || 3 == concate_image_count)
        {
            model_inference_result_rotation_center.x = int(model_inference_result_show_resize.cols *
                                                           (infer_distance_left / (infer_distance_left + infer_distance_right)));
            model_inference_result_rotation_center.y = int(model_inference_result_show_resize.rows);
        }
        else
        {
            model_inference_result_rotation_center.x = int(model_inference_result_show_resize.cols *
                                                           (infer_distance_left / (infer_distance_left + infer_distance_right)));
            model_inference_result_rotation_center.y = int(model_inference_result_show_resize.rows *
                                                           (infer_distance_front / (infer_distance_front + infer_distance_back)));
        }
        drawCrossMark(model_inference_result_show_resize, model_inference_result_rotation_center, model_type);

        temp_cv_show_image.image = model_inference_result_show_resize;
        temp_cv_show_image.encoding = "mono8";
        temp_cv_show_image.header = image_message_vect[0]->header;
        temp_cv_show_image.toImageMsg(temp_ros_show_image);
        pub_cross_view_result_show_image.publish(temp_ros_show_image);
    }

    // publish model inference result
    cv_bridge::CvImage temp_cv_image;
    sensor_msgs::Image temp_ros_image;

    temp_cv_image.image = model_inference_result;
    temp_cv_image.encoding = "mono8";
    temp_cv_image.header = image_message_vect[0]->header;
    temp_cv_image.toImageMsg(temp_ros_image);
    pub_cross_view_result_image.publish(temp_ros_image);

    std::stringstream last_frame_timestamp_stringstream;
    last_frame_timestamp_stringstream << image_message_vect[0]->header.stamp.sec << "." << image_message_vect[0]->header.stamp.nsec;
    last_frame_timestamp_stringstream >> last_frame_timestamp;

    endTime_0 = clock(); //计时结束
    std::cout << "The run time is:" << (double)(endTime_0 - startTime_0) / CLOCKS_PER_SEC << "s" << std::endl;
    std::cout << "******************************************" << std::endl;
    // std::cout << "The total run time is:" << (double)clock() / CLOCKS_PER_SEC << "s" << std::endl;

    return;
}

void Camera_cross_view_base_opencv_gpu::PrepareImage_concate(std::vector<cv::Mat> image_vect, float *gpu_input)
{
    PrepareImage_startTime_0 = clock(); //计时开始

    std::vector<cv::cuda::GpuMat> image_vect_gpu;
    auto net_input_image_size = cv::Size(net_input_width_, net_input_height_);
    int channelLength = net_input_image_size.area();

    std::vector<float> result(net_input_height_ * net_input_width_ * net_input_channel_ * net_input_batch_size_);

    cv::cuda::GpuMat concate_image_gpu(net_input_image_size,
                                       CV_8UC3,
                                       cv::Scalar(0, 0, 0));
    cv::Mat concate_image_cpu(net_input_image_size,
                              CV_8UC3,
                              cv::Scalar(0, 0, 0));

    if (1 == image_vect.size())
    {
        cv::cuda::GpuMat temp_concate_image_gpu(net_input_image_size,
                                                CV_8UC3,
                                                cv::Scalar(0, 0, 0));
        cv::cuda::GpuMat concate_image_gpu(net_input_image_size,
                                           CV_8UC3,
                                           cv::Scalar(0, 0, 0));

        std::vector<cv::Mat> image_vect_replace;
        image_vect_replace.push_back(image_vect[0]);



        for (size_t i = 0; i < image_vect_replace.size(); i++)
        {
            auto gpu_image_size = cv::Size(int(net_input_width_), int(net_input_height_));
            cv::cuda::GpuMat image_gpu;
            cv::cuda::GpuMat image_resize_gpu;
            cv::Mat cpu_image_resize;
            
            image_gpu.upload(image_vect_replace[i]);
            
            if (delete_sky)
            {
                auto image_gpu_delete_sky_size = cv::Size(image_gpu.cols, int(image_gpu.rows * (1 - delete_sky_proportion)));
                cv::cuda::GpuMat image_gpu_delete_sky(image_gpu_delete_sky_size,
                                                      CV_8UC3,
                                                      cv::Scalar(0, 0, 0));
                image_gpu(cv::Rect(0, image_gpu.rows - image_gpu_delete_sky_size.height,
                                   image_gpu.cols, image_gpu_delete_sky_size.height))
                    .copyTo(image_gpu_delete_sky(cv::Rect(0, 0,
                                                          image_gpu_delete_sky.cols, image_gpu_delete_sky_size.height)));
                image_gpu = image_gpu_delete_sky;
            }

            // resize
            cv::cuda::resize(image_gpu, image_resize_gpu, gpu_image_size);
            temp_concate_image_gpu = image_resize_gpu.clone();
        }

        cv::cuda::resize(temp_concate_image_gpu, concate_image_gpu, net_input_image_size);
        concate_image_gpu.download(concate_image_cpu);

        PrepareImage_concate_resize_endTime_0 = clock();
        std::cout << "Image resize and concate time is:" << (double)(PrepareImage_concate_resize_endTime_0 - PrepareImage_concate_resize_startTime_0) / CLOCKS_PER_SEC << "s" << std::endl;

        // mask self car
        PrepareImage_mask_startTime_0 = clock();
        std::vector<std::vector<cv::Point>> mask_lines;
        if (0 == input_concate_image_mask_car_name)
        {

            if (delete_sky)
            {
                // front center
                int delete_sky_mask_offset_height = int(net_input_image_size.height * delete_sky_proportion);
                double delete_sky_mask_offset_height_scale = 1 / (1 - delete_sky_proportion);
                std::vector<cv::Point> mask_line_0{
                    cv::Point(int(0 * input_mask_scale), int(((964 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(71 * input_mask_scale), int((943 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale)),
                    cv::Point(int(171 * input_mask_scale), int((931 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale)),
                    cv::Point(int(335 * input_mask_scale), int((939 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale)),
                    cv::Point(int(453 * input_mask_scale), int((948 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale)),
                    cv::Point(int(452 * input_mask_scale), int((850 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale)),
                    cv::Point(int(469 * input_mask_scale), int((796 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale)),
                    cv::Point(int(726 * input_mask_scale), int((789 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale)),
                    cv::Point(int(772 * input_mask_scale), int((837 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale)),
                    cv::Point(int(773 * input_mask_scale), int((934 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale)),
                    cv::Point(int(1023 * input_mask_scale), int((897 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale)),
                    cv::Point(int(1023 * input_mask_scale), int(1023 * input_mask_scale)),
                    cv::Point(int(0 * input_mask_scale), int(1023 * input_mask_scale))};
                mask_lines.push_back(mask_line_0);
                // front right
                // std::vector<cv::Point> mask_line_0{
                //     cv::Point(int(0 * input_mask_scale), int(475 * input_mask_scale * (1 - delete_sky_proportion))),
                //     cv::Point(int(120 * input_mask_scale), int(680 * input_mask_scale * (1 - delete_sky_proportion))),
                //     cv::Point(int(130 * input_mask_scale), int(682 * input_mask_scale * (1 - delete_sky_proportion))),
                //     cv::Point(int(168 * input_mask_scale), int(713 * input_mask_scale * (1 - delete_sky_proportion))),
                //     cv::Point(int(280 * input_mask_scale), int(733 * input_mask_scale * (1 - delete_sky_proportion))),
                //     cv::Point(int(303 * input_mask_scale), int(744 * input_mask_scale * (1 - delete_sky_proportion))),
                //     cv::Point(int(672 * input_mask_scale), int(723 * input_mask_scale * (1 - delete_sky_proportion))),
                //     cv::Point(int(736 * input_mask_scale), int(750 * input_mask_scale * (1 - delete_sky_proportion))),
                //     cv::Point(int(885 * input_mask_scale), int(722 * input_mask_scale * (1 - delete_sky_proportion))),
                //     cv::Point(int(1023 * input_mask_scale), int(550 * input_mask_scale * (1 - delete_sky_proportion))),
                //     cv::Point(int(1023 * input_mask_scale), int(1023 * input_mask_scale * (1 - delete_sky_proportion))),
                //     cv::Point(int(0 * input_mask_scale), int(1023 * input_mask_scale * (1 - delete_sky_proportion)))};
                // mask_lines.push_back(mask_line_0);
            }
            else
            {
                std::vector<cv::Point> mask_line_0{
                    cv::Point(int(0 * input_mask_scale), int(964 * input_mask_scale)),
                    cv::Point(int(71 * input_mask_scale), int(943 * input_mask_scale)),
                    cv::Point(int(171 * input_mask_scale), int(931 * input_mask_scale)),
                    cv::Point(int(335 * input_mask_scale), int(939 * input_mask_scale)),
                    cv::Point(int(453 * input_mask_scale), int(948 * input_mask_scale)),
                    cv::Point(int(452 * input_mask_scale), int(850 * input_mask_scale)),
                    cv::Point(int(469 * input_mask_scale), int(796 * input_mask_scale)),
                    cv::Point(int(726 * input_mask_scale), int(789 * input_mask_scale)),
                    cv::Point(int(772 * input_mask_scale), int(837 * input_mask_scale)),
                    cv::Point(int(773 * input_mask_scale), int(934 * input_mask_scale)),
                    cv::Point(int(1023 * input_mask_scale), int(897 * input_mask_scale)),
                    cv::Point(int(1023 * input_mask_scale), int(1023 * input_mask_scale)),
                    cv::Point(int(0 * input_mask_scale), int(1023 * input_mask_scale))};
                mask_lines.push_back(mask_line_0);
                // front right
                // std::vector<cv::Point> mask_line_0{
                //     cv::Point(int(0 * input_mask_scale), int(475 * input_mask_scale)),
                //     cv::Point(int(120 * input_mask_scale), int(680 * input_mask_scale)),
                //     cv::Point(int(130 * input_mask_scale), int(682 * input_mask_scale)),
                //     cv::Point(int(168 * input_mask_scale), int(713 * input_mask_scale)),
                //     cv::Point(int(280 * input_mask_scale), int(733 * input_mask_scale)),
                //     cv::Point(int(303 * input_mask_scale), int(744 * input_mask_scale)),
                //     cv::Point(int(672 * input_mask_scale), int(723 * input_mask_scale)),
                //     cv::Point(int(736 * input_mask_scale), int(750 * input_mask_scale)),
                //     cv::Point(int(885 * input_mask_scale), int(722 * input_mask_scale)),
                //     cv::Point(int(1023 * input_mask_scale), int(550 * input_mask_scale)),
                //     cv::Point(int(1023 * input_mask_scale), int(1023 * input_mask_scale)),
                //     cv::Point(int(0 * input_mask_scale), int(1023 * input_mask_scale))};
                // mask_lines.push_back(mask_line_0);
            }
        }
        if (1 == input_concate_image_mask_car_name)
        {
            // front center
            int delete_sky_mask_offset_height = int(net_input_image_size.height * delete_sky_proportion);
            double delete_sky_mask_offset_height_scale = 1 / (1 - delete_sky_proportion);
            if (delete_sky)
            {
                std::vector<cv::Point> mask_line_0{
                    cv::Point(int(342 * input_mask_scale), int(((364 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(412 * input_mask_scale), int(((324 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(581 * input_mask_scale), int(((345 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(634 * input_mask_scale), int(((440 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(681 * input_mask_scale), int(((413 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(681 * input_mask_scale), int(511 * input_mask_scale * (1 - delete_sky_proportion))),
                    cv::Point(int(342 * input_mask_scale), int(511 * input_mask_scale * (1 - delete_sky_proportion)))};
                mask_lines.push_back(mask_line_0);
            }
            else
            {
                std::vector<cv::Point> mask_line_0{
                    cv::Point(int(342 * input_mask_scale), int(364 * input_mask_scale)),
                    cv::Point(int(412 * input_mask_scale), int(324 * input_mask_scale)),
                    cv::Point(int(581 * input_mask_scale), int(345 * input_mask_scale)),
                    cv::Point(int(634 * input_mask_scale), int(440 * input_mask_scale)),
                    cv::Point(int(681 * input_mask_scale), int(413 * input_mask_scale)),
                    cv::Point(int(681 * input_mask_scale), int(511 * input_mask_scale)),
                    cv::Point(int(342 * input_mask_scale), int(511 * input_mask_scale))};
                mask_lines.push_back(mask_line_0);
            }
        }

        cv::drawContours(concate_image_cpu, mask_lines, -1, cv::Scalar(0, 0, 0), -1);

        PrepareImage_mask_endTime_0 = clock();
        std::cout << "Mask self car time is:" << (double)(PrepareImage_mask_endTime_0 - PrepareImage_mask_startTime_0) / CLOCKS_PER_SEC << "s" << std::endl;

        // set input image
        // cv::Mat input_image;
        // input_image = cv::imread("/mnt/data_1/Dataset/dataset_sample/Autopilot_bev_dataset_sample/input/0401_2_000147.jpg");
        // cv::resize(input_image, image_resize, cv::Size(net_input_width_, net_input_height_));
        // cv::imshow("source image", image_resize);
        // cv::waitKey(0);

        // pub input net image
        if (show_result)
        {
            cv_bridge::CvImage input_net_cv_image;
            sensor_msgs::Image input_net_image;

            input_net_cv_image.image = concate_image_cpu;
            input_net_cv_image.encoding = "bgr8";
            input_net_cv_image.toImageMsg(input_net_image);
            pub_cross_view_input_net_image.publish(input_net_image);
        }

        PrepareImage_memcpy_startTime_0 = clock();

        cv::cuda::GpuMat gpu_image_resize;
        cv::cuda::GpuMat gpu_image_float;
        gpu_image_resize.upload(concate_image_cpu);
        gpu_image_resize.convertTo(gpu_image_float, CV_32FC3, 1.f / 255.f);

        std::vector<cv::cuda::GpuMat> chw;

        for (int i = 0; i < net_input_channel_; ++i)
            chw.emplace_back(cv::cuda::GpuMat(net_input_image_size,
                                              CV_32FC1, gpu_input + i * net_input_width_ * net_input_height_));
        cv::cuda::split(gpu_image_float, chw);

        PrepareImage_memcpy_endTime_0 = clock();
        std::cout << "Memcpy time is:" << (double)(PrepareImage_memcpy_endTime_0 - PrepareImage_memcpy_startTime_0) / CLOCKS_PER_SEC << "s" << std::endl;
    }
    if (3 == image_vect.size())
    {
        cv::cuda::GpuMat temp_concate_image_gpu(cv::Size(1023, 1024),
                                                CV_8UC3,
                                                cv::Scalar(0, 0, 0));
        cv::cuda::GpuMat concate_image_gpu(cv::Size(1023, 1024),
                                           CV_8UC3,
                                           cv::Scalar(0, 0, 0));

        std::vector<cv::Mat> image_vect_replace;
        image_vect_replace.push_back(image_vect[1]);
        image_vect_replace.push_back(image_vect[0]);
        image_vect_replace.push_back(image_vect[3]);

        PrepareImage_concate_resize_startTime_0 = clock();

        for (size_t i = 0; i < image_vect_replace.size(); i++)
        {
            auto gpu_image_size = cv::Size(int(net_input_width_ / 3), int(net_input_height_));
            cv::cuda::GpuMat image_gpu;
            cv::cuda::GpuMat image_resize_gpu;
            cv::Mat cpu_image_resize;
            image_gpu.upload(image_vect_replace[i]);

            // resize
            cv::cuda::resize(image_gpu, image_resize_gpu, gpu_image_size);

            // concate image
            if (0 == i)
            {
                image_resize_gpu.copyTo(temp_concate_image_gpu(cv::Rect(0, 0,
                                                                        image_resize_gpu.cols, image_resize_gpu.rows)));
            }
            if (1 <= i)
            {
                image_resize_gpu.copyTo(temp_concate_image_gpu(cv::Rect((image_resize_gpu.cols * i - 1), 0,
                                                                        image_resize_gpu.cols, image_resize_gpu.rows)));
            }
        }
        cv::cuda::resize(temp_concate_image_gpu, concate_image_gpu, net_input_image_size);
        concate_image_gpu.download(concate_image_cpu);

        PrepareImage_concate_resize_endTime_0 = clock();
        std::cout << "Image resize and concate time is:" << (double)(PrepareImage_concate_resize_endTime_0 - PrepareImage_concate_resize_startTime_0) / CLOCKS_PER_SEC << "s" << std::endl;

        // mask self car
        PrepareImage_mask_startTime_0 = clock();

        std::vector<std::vector<cv::Point>> mask_lines;
        if (0 == input_concate_image_mask_car_name)
        {
            std::vector<cv::Point> mask_line_0{
                cv::Point(0, 0),
                cv::Point(10, 0),
                cv::Point(25, 790),
                cv::Point(161, 1024),
                cv::Point(280, 1024),
                cv::Point(300, 700),
                cv::Point(315, 560),
                cv::Point(334, 536),
                cv::Point(341, 540),
                cv::Point(341, 1024),
                cv::Point(0, 1024),
                cv::Point(0, 0)};
            std::vector<cv::Point> mask_line_1{
                cv::Point(341, 728),
                cv::Point(424, 650),
                cv::Point(555, 688),
                cv::Point(570, 706),
                cv::Point(645, 912),
                cv::Point(682, 862),
                cv::Point(682, 1024),
                cv::Point(341, 1024)};
            std::vector<cv::Point> mask_line_2{
                cv::Point(682, 646),
                cv::Point(716, 1024),
                cv::Point(865, 1024),
                cv::Point(995, 788),
                cv::Point(1007, 0),
                cv::Point(1024, 0),
                cv::Point(1024, 1024),
                cv::Point(682, 1024)};
            mask_lines.push_back(mask_line_0);
            mask_lines.push_back(mask_line_1);
            mask_lines.push_back(mask_line_2);
        }
        if (1 == input_concate_image_mask_car_name)
        {
            std::vector<cv::Point> mask_line_0{
                cv::Point(0, 0),
                cv::Point(10, 0),
                cv::Point(25, 790),
                cv::Point(161, 1024),
                cv::Point(280, 1024),
                cv::Point(300, 700),
                cv::Point(315, 560),
                cv::Point(334, 536),
                cv::Point(341, 540),
                cv::Point(341, 1024),
                cv::Point(0, 1024),
                cv::Point(0, 0)};
            std::vector<cv::Point> mask_line_1{
                cv::Point(341, 728),
                cv::Point(424, 650),
                cv::Point(555, 688),
                cv::Point(570, 706),
                cv::Point(645, 912),
                cv::Point(682, 862),
                cv::Point(682, 1024),
                cv::Point(341, 1024)};
            std::vector<cv::Point> mask_line_2{
                cv::Point(682, 646),
                cv::Point(716, 1024),
                cv::Point(865, 1024),
                cv::Point(995, 788),
                cv::Point(1007, 0),
                cv::Point(1024, 0),
                cv::Point(1024, 1024),
                cv::Point(682, 1024)};
            mask_lines.push_back(mask_line_0);
            mask_lines.push_back(mask_line_1);
            mask_lines.push_back(mask_line_2);
        }
        cv::drawContours(concate_image_cpu, mask_lines, -1, cv::Scalar(0, 0, 0), -1);

        PrepareImage_mask_endTime_0 = clock();
        std::cout << "Mask self car time is:" << (double)(PrepareImage_mask_endTime_0 - PrepareImage_mask_startTime_0) / CLOCKS_PER_SEC << "s" << std::endl;

        // set input image
        // cv::Mat input_image;
        // input_image = cv::imread("/mnt/data_1/Dataset/dataset_sample/Autopilot_bev_dataset_sample/input/0401_2_000147.jpg");
        // cv::resize(input_image, image_resize, cv::Size(net_input_width_, net_input_height_));
        // cv::imshow("source image", image_resize);
        // cv::waitKey(0);

        // pub input net image
        if (show_result)
        {
            cv_bridge::CvImage input_net_cv_image;
            sensor_msgs::Image input_net_image;

            input_net_cv_image.image = concate_image_cpu;
            input_net_cv_image.encoding = "bgr8";
            input_net_cv_image.toImageMsg(input_net_image);
            pub_cross_view_input_net_image.publish(input_net_image);
        }

        PrepareImage_memcpy_startTime_0 = clock();

        cv::cuda::GpuMat gpu_image_resize;
        cv::cuda::GpuMat gpu_image_float;
        gpu_image_resize.upload(concate_image_cpu);
        gpu_image_resize.convertTo(gpu_image_float, CV_32FC3, 1.f / 255.f);

        std::vector<cv::cuda::GpuMat> chw;

        for (int i = 0; i < net_input_channel_; ++i)
            chw.emplace_back(cv::cuda::GpuMat(net_input_image_size,
                                              CV_32FC1, gpu_input + i * net_input_width_ * net_input_height_));
        cv::cuda::split(gpu_image_float, chw);

        PrepareImage_memcpy_endTime_0 = clock();
        std::cout << "Memcpy time is:" << (double)(PrepareImage_memcpy_endTime_0 - PrepareImage_memcpy_startTime_0) / CLOCKS_PER_SEC << "s" << std::endl;
    }
    if (6 == image_vect.size())
    {
        cv::cuda::GpuMat temp_concate_image_gpu(cv::Size(int(net_input_width_/3)*3, net_input_height_),
                                                CV_8UC3,
                                                cv::Scalar(0, 0, 0));
        cv::cuda::GpuMat concate_image_gpu(net_input_image_size,
                                           CV_8UC3,
                                           cv::Scalar(0, 0, 0));

        std::vector<cv::Mat> image_vect_replace;
        image_vect_replace.push_back(image_vect[1]);
        image_vect_replace.push_back(image_vect[0]);
        image_vect_replace.push_back(image_vect[3]);
        image_vect_replace.push_back(image_vect[2]);
        image_vect_replace.push_back(image_vect[5]);
        image_vect_replace.push_back(image_vect[4]);

        PrepareImage_concate_resize_startTime_0 = clock();

        for (size_t i = 0; i < image_vect_replace.size(); i++)
        {
            auto gpu_image_size = cv::Size(int(net_input_width_ / 3), int(net_input_height_ / 2));
            cv::cuda::GpuMat image_gpu;
            cv::cuda::GpuMat image_resize_gpu;
            cv::Mat cpu_image_resize;
            image_gpu.upload(image_vect_replace[i]);

            if (delete_sky)
            {
                auto image_gpu_delete_sky_size = cv::Size(image_gpu.cols, int(image_gpu.rows * (1 - delete_sky_proportion)));
                cv::cuda::GpuMat image_gpu_delete_sky(image_gpu_delete_sky_size,
                                                      CV_8UC3,
                                                      cv::Scalar(0, 0, 0));
                image_gpu(cv::Rect(0, image_gpu.rows - image_gpu_delete_sky_size.height,
                                   image_gpu.cols, image_gpu_delete_sky_size.height))
                    .copyTo(image_gpu_delete_sky(cv::Rect(0, 0,
                                                          image_gpu_delete_sky.cols, image_gpu_delete_sky_size.height)));
                image_gpu = image_gpu_delete_sky;
            }

            // resize
            cv::cuda::resize(image_gpu, image_resize_gpu, gpu_image_size);

            // concate image
            if (0 == i)
            {
                image_resize_gpu.copyTo(temp_concate_image_gpu(cv::Rect(0, 0,
                                                                        image_resize_gpu.cols, image_resize_gpu.rows)));
            }
            else if (1 <= i && i <= 2)
            {
                image_resize_gpu.copyTo(temp_concate_image_gpu(cv::Rect((image_resize_gpu.cols * i - 1), 0,
                                                                        image_resize_gpu.cols, image_resize_gpu.rows)));
            }
            else if (3 == i)
            {
                image_resize_gpu.copyTo(temp_concate_image_gpu(cv::Rect(0, image_resize_gpu.rows,
                                                                        image_resize_gpu.cols, image_resize_gpu.rows)));
            }
            else if (i > 3)
            {
                image_resize_gpu.copyTo(temp_concate_image_gpu(cv::Rect((image_resize_gpu.cols * (i - 3) - 1), image_resize_gpu.rows,
                                                                        image_resize_gpu.cols, image_resize_gpu.rows)));
            }
        }
        cv::cuda::resize(temp_concate_image_gpu, concate_image_gpu, net_input_image_size);
        concate_image_gpu.download(concate_image_cpu);


        PrepareImage_concate_resize_endTime_0 = clock();
        std::cout << "Image resize and concate time is:" << (double)(PrepareImage_concate_resize_endTime_0 - PrepareImage_concate_resize_startTime_0) / CLOCKS_PER_SEC << "s" << std::endl;

        // mask self car
        PrepareImage_mask_startTime_0 = clock();
        std::vector<std::vector<cv::Point>> mask_lines;

        if (delete_sky)
        {
            if (0 == input_concate_image_mask_car_name)
            {
                int delete_sky_mask_offset_height = int(net_input_image_size.height * 0.5 * delete_sky_proportion);
                double delete_sky_mask_offset_height_scale = 1 / (1 - delete_sky_proportion);
                std::vector<cv::Point> mask_line_0_0{
                    cv::Point(int(0 * input_mask_scale), int(0 * input_mask_scale)),
                    cv::Point(int(8 * input_mask_scale), int(0 * input_mask_scale)),
                    cv::Point(int(27 * input_mask_scale), int(((427 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(98 * input_mask_scale), int(511 * input_mask_scale)),
                    cv::Point(int(0 * input_mask_scale), int(511 * input_mask_scale)),
                    cv::Point(int(0 * input_mask_scale), int(0 * input_mask_scale))};
                std::vector<cv::Point> mask_line_0_1{
                    cv::Point(int(273 * input_mask_scale), int(511 * input_mask_scale)),
                    cv::Point(int(290 * input_mask_scale), int(((376 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(308 * input_mask_scale), int(((292 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(321 * input_mask_scale), int(((287 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(327 * input_mask_scale), int(((280 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(336 * input_mask_scale), int(((280 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(341 * input_mask_scale), int(((281 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(341 * input_mask_scale), int(511 * input_mask_scale))};
                // front center
                std::vector<cv::Point> mask_line_1{
                    cv::Point(int(341 * input_mask_scale), int(((482 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(360 * input_mask_scale), int(((473 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(395 * input_mask_scale), int(((466 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(446 * input_mask_scale), int(((469 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(492 * input_mask_scale), int(((474 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(492 * input_mask_scale), int(((424 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(497 * input_mask_scale), int(((398 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(583 * input_mask_scale), int(((395 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(598 * input_mask_scale), int(((419 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(599 * input_mask_scale), int(((467 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(681 * input_mask_scale), int(((449 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(681 * input_mask_scale), int(511 * input_mask_scale)),
                    cv::Point(int(341 * input_mask_scale), int(511 * input_mask_scale))};
                // front right
                // std::vector<cv::Point> mask_line_1{
                //     cv::Point(int(342 * input_mask_scale), int(237 * input_mask_scale)),
                //     cv::Point(int(385 * input_mask_scale), int(340 * input_mask_scale)),
                //     cv::Point(int(398 * input_mask_scale), int(356 * input_mask_scale)),
                //     cv::Point(int(433 * input_mask_scale), int(365 * input_mask_scale)),
                //     cv::Point(int(433 * input_mask_scale), int(371 * input_mask_scale)),
                //     cv::Point(int(565 * input_mask_scale), int(360 * input_mask_scale)),
                //     cv::Point(int(587 * input_mask_scale), int(374 * input_mask_scale)),
                //     cv::Point(int(636 * input_mask_scale), int(358 * input_mask_scale)),
                //     cv::Point(int(681 * input_mask_scale), int(274 * input_mask_scale)),
                //     cv::Point(int(681 * input_mask_scale), int(511 * input_mask_scale)),
                //     cv::Point(int(342 * input_mask_scale), int(511 * input_mask_scale))};
                std::vector<cv::Point> mask_line_2_0{
                    cv::Point(int(682 * input_mask_scale), int(((293 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(689 * input_mask_scale), int(((305 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(702 * input_mask_scale), int(((310 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(730 * input_mask_scale), int(511 * input_mask_scale)),
                    cv::Point(int(682 * input_mask_scale), int(511 * input_mask_scale))};
                std::vector<cv::Point> mask_line_2_1{
                    cv::Point(int(920 * input_mask_scale), int(511 * input_mask_scale)),
                    cv::Point(int(967 * input_mask_scale), int(((471 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(1012 * input_mask_scale), int(((416 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(1023 * input_mask_scale), int(((295 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(1023 * input_mask_scale), int(511 * input_mask_scale))};
                std::vector<cv::Point> mask_line_3{
                    cv::Point(int(0 * input_mask_scale), int(((877 - 511) * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale) + 511),
                    cv::Point(int(8 * input_mask_scale), int(((900 - 511) * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale) + 511),
                    cv::Point(int(16 * input_mask_scale), int(((910 - 511) * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale) + 511),
                    cv::Point(int(28 * input_mask_scale), int(((943 - 511) * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale) + 511),
                    cv::Point(int(35 * input_mask_scale), int(((981 - 511) * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale) + 511),
                    cv::Point(int(96 * input_mask_scale), int(1023 * input_mask_scale)),
                    cv::Point(int(0 * input_mask_scale), int(1023 * input_mask_scale))};
                std::vector<cv::Point> mask_line_5{
                    cv::Point(int(912 * input_mask_scale), int(1023 * input_mask_scale)),
                    cv::Point(int(932 * input_mask_scale), int(((999 - 511) * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale) + 511),
                    cv::Point(int(975 * input_mask_scale), int(((975 - 511) * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale) + 511),
                    cv::Point(int(985 * input_mask_scale), int(((903 - 511) * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale) + 511),
                    cv::Point(int(997 * input_mask_scale), int(((858 - 511) * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale) + 511),
                    cv::Point(int(1006 * input_mask_scale), int(((844 - 511) * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale) + 511),
                    cv::Point(int(1023 * input_mask_scale), int(((793 - 511) * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale) + 511),
                    cv::Point(int(1023 * input_mask_scale), int(1023 * input_mask_scale))};
                mask_lines.push_back(mask_line_0_0);
                mask_lines.push_back(mask_line_0_1);
                mask_lines.push_back(mask_line_1);
                mask_lines.push_back(mask_line_2_0);
                mask_lines.push_back(mask_line_2_1);
                mask_lines.push_back(mask_line_3);
                mask_lines.push_back(mask_line_5);
            }
            if (1 == input_concate_image_mask_car_name)
            {
                int delete_sky_mask_offset_height = int(net_input_image_size.height * 0.5 * delete_sky_proportion);
                double delete_sky_mask_offset_height_scale = 1 / (1 - delete_sky_proportion);
                std::vector<cv::Point> mask_line_0_0{
                    cv::Point(int(0 * input_mask_scale), int(0 * input_mask_scale)),
                    cv::Point(int(10 * input_mask_scale), int(0 * input_mask_scale)),
                    cv::Point(int(25 * input_mask_scale), int(((395 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(138 * input_mask_scale), int(511 * input_mask_scale)),
                    cv::Point(int(0 * input_mask_scale), int(511 * input_mask_scale))};
                std::vector<cv::Point> mask_line_0_1{
                    cv::Point(int(280 * input_mask_scale), int(511 * input_mask_scale)),
                    cv::Point(int(300 * input_mask_scale), int(((325 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(315 * input_mask_scale), int(((280 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(341 * input_mask_scale), int(((263 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(341 * input_mask_scale), int(511 * input_mask_scale))};
                std::vector<cv::Point> mask_line_1{
                    cv::Point(int(342 * input_mask_scale), int(((364 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(412 * input_mask_scale), int(((324 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(581 * input_mask_scale), int(((345 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(634 * input_mask_scale), int(((440 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(681 * input_mask_scale), int(((413 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(681 * input_mask_scale), int(511 * input_mask_scale)),
                    cv::Point(int(342 * input_mask_scale), int(511 * input_mask_scale))};
                std::vector<cv::Point> mask_line_2_0{
                    cv::Point(int(682 * input_mask_scale), int(((320 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(701 * input_mask_scale), int(((387 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(716 * input_mask_scale), int(511 * input_mask_scale)),
                    cv::Point(int(682 * input_mask_scale), int(511 * input_mask_scale))};
                std::vector<cv::Point> mask_line_2_1{
                    cv::Point(int(893 * input_mask_scale), int(511 * input_mask_scale)),
                    cv::Point(int(1000 * input_mask_scale), int(((392 * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale))),
                    cv::Point(int(1008 * input_mask_scale), int(0 * input_mask_scale)),
                    cv::Point(int(1023 * input_mask_scale), int(0 * input_mask_scale)),
                    cv::Point(int(1023 * input_mask_scale), int(511 * input_mask_scale))};
                std::vector<cv::Point> mask_line_3{
                    cv::Point(int(0 * input_mask_scale), int(((886 - 511) * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale) + 511),
                    cv::Point(int(26 * input_mask_scale), int(((936 - 511) * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale) + 511),
                    cv::Point(int(30 * input_mask_scale), int(((990 - 511) * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale) + 511),
                    cv::Point(int(100 * input_mask_scale), int(1023 * input_mask_scale)),
                    cv::Point(int(0 * input_mask_scale), int(1023 * input_mask_scale))};
                std::vector<cv::Point> mask_line_5{
                    cv::Point(int(902 * input_mask_scale), int(1023 * input_mask_scale)),
                    cv::Point(int(965 * input_mask_scale), int(((960 - 511) * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale) + 511),
                    cv::Point(int(980 * input_mask_scale), int(((873 - 511) * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale) + 511),
                    cv::Point(int(1023 * input_mask_scale), int(((750 - 511) * input_mask_scale - delete_sky_mask_offset_height) * delete_sky_mask_offset_height_scale) + 511),
                    cv::Point(int(1023 * input_mask_scale), int(1023 * input_mask_scale))};
                mask_lines.push_back(mask_line_0_0);
                mask_lines.push_back(mask_line_0_1);
                mask_lines.push_back(mask_line_1);
                mask_lines.push_back(mask_line_2_0);
                mask_lines.push_back(mask_line_2_1);
                mask_lines.push_back(mask_line_3);
                mask_lines.push_back(mask_line_5);
            }
        }
        else
        {
            if (0 == input_concate_image_mask_car_name)
            {
                std::vector<cv::Point> mask_line_0_0{
                    cv::Point(int(0 * input_mask_scale), int(0 * input_mask_scale)),
                    cv::Point(int(8 * input_mask_scale), int(0 * input_mask_scale)),
                    cv::Point(int(27 * input_mask_scale), int(427 * input_mask_scale)),
                    cv::Point(int(98 * input_mask_scale), int(511 * input_mask_scale)),
                    cv::Point(int(0 * input_mask_scale), int(511 * input_mask_scale)),
                    cv::Point(int(0 * input_mask_scale), int(0 * input_mask_scale))};
                std::vector<cv::Point> mask_line_0_1{
                    cv::Point(int(273 * input_mask_scale), int(511 * input_mask_scale)),
                    cv::Point(int(290 * input_mask_scale), int(376 * input_mask_scale)),
                    cv::Point(int(308 * input_mask_scale), int(292 * input_mask_scale)),
                    cv::Point(int(321 * input_mask_scale), int(287 * input_mask_scale)),
                    cv::Point(int(327 * input_mask_scale), int(280 * input_mask_scale)),
                    cv::Point(int(336 * input_mask_scale), int(280 * input_mask_scale)),
                    cv::Point(int(341 * input_mask_scale), int(281 * input_mask_scale)),
                    cv::Point(int(341 * input_mask_scale), int(511 * input_mask_scale))};
                // front center
                std::vector<cv::Point> mask_line_1{
                    cv::Point(int(341 * input_mask_scale), int(482 * input_mask_scale)),
                    cv::Point(int(360 * input_mask_scale), int(473 * input_mask_scale)),
                    cv::Point(int(395 * input_mask_scale), int(466 * input_mask_scale)),
                    cv::Point(int(446 * input_mask_scale), int(469 * input_mask_scale)),
                    cv::Point(int(492 * input_mask_scale), int(474 * input_mask_scale)),
                    cv::Point(int(492 * input_mask_scale), int(424 * input_mask_scale)),
                    cv::Point(int(497 * input_mask_scale), int(398 * input_mask_scale)),
                    cv::Point(int(583 * input_mask_scale), int(395 * input_mask_scale)),
                    cv::Point(int(598 * input_mask_scale), int(419 * input_mask_scale)),
                    cv::Point(int(599 * input_mask_scale), int(467 * input_mask_scale)),
                    cv::Point(int(681 * input_mask_scale), int(449 * input_mask_scale)),
                    cv::Point(int(681 * input_mask_scale), int(511 * input_mask_scale)),
                    cv::Point(int(341 * input_mask_scale), int(511 * input_mask_scale))};
                // front right
                // std::vector<cv::Point> mask_line_1{
                //     cv::Point(int(342 * input_mask_scale), int(237 * input_mask_scale)),
                //     cv::Point(int(385 * input_mask_scale), int(340 * input_mask_scale)),
                //     cv::Point(int(398 * input_mask_scale), int(356 * input_mask_scale)),
                //     cv::Point(int(433 * input_mask_scale), int(365 * input_mask_scale)),
                //     cv::Point(int(433 * input_mask_scale), int(371 * input_mask_scale)),
                //     cv::Point(int(565 * input_mask_scale), int(360 * input_mask_scale)),
                //     cv::Point(int(587 * input_mask_scale), int(374 * input_mask_scale)),
                //     cv::Point(int(636 * input_mask_scale), int(358 * input_mask_scale)),
                //     cv::Point(int(681 * input_mask_scale), int(274 * input_mask_scale)),
                //     cv::Point(int(681 * input_mask_scale), int(511 * input_mask_scale)),
                //     cv::Point(int(342 * input_mask_scale), int(511 * input_mask_scale))};
                std::vector<cv::Point> mask_line_2_0{
                    cv::Point(int(682 * input_mask_scale), int(293 * input_mask_scale)),
                    cv::Point(int(689 * input_mask_scale), int(305 * input_mask_scale)),
                    cv::Point(int(702 * input_mask_scale), int(310 * input_mask_scale)),
                    cv::Point(int(730 * input_mask_scale), int(511 * input_mask_scale)),
                    cv::Point(int(682 * input_mask_scale), int(511 * input_mask_scale))};
                std::vector<cv::Point> mask_line_2_1{
                    cv::Point(int(920 * input_mask_scale), int(511 * input_mask_scale)),
                    cv::Point(int(967 * input_mask_scale), int(471 * input_mask_scale)),
                    cv::Point(int(1012 * input_mask_scale), int(416 * input_mask_scale)),
                    cv::Point(int(1023 * input_mask_scale), int(295 * input_mask_scale)),
                    cv::Point(int(1023 * input_mask_scale), int(511 * input_mask_scale))};
                std::vector<cv::Point> mask_line_3{
                    cv::Point(int(0 * input_mask_scale), int(877 * input_mask_scale)),
                    cv::Point(int(8 * input_mask_scale), int(900 * input_mask_scale)),
                    cv::Point(int(16 * input_mask_scale), int(910 * input_mask_scale)),
                    cv::Point(int(28 * input_mask_scale), int(943 * input_mask_scale)),
                    cv::Point(int(35 * input_mask_scale), int(981 * input_mask_scale)),
                    cv::Point(int(96 * input_mask_scale), int(1023 * input_mask_scale)),
                    cv::Point(int(0 * input_mask_scale), int(1023 * input_mask_scale))};
                std::vector<cv::Point> mask_line_5{
                    cv::Point(int(912 * input_mask_scale), int(1023 * input_mask_scale)),
                    cv::Point(int(932 * input_mask_scale), int(999 * input_mask_scale)),
                    cv::Point(int(975 * input_mask_scale), int(975 * input_mask_scale)),
                    cv::Point(int(985 * input_mask_scale), int(903 * input_mask_scale)),
                    cv::Point(int(997 * input_mask_scale), int(858 * input_mask_scale)),
                    cv::Point(int(1006 * input_mask_scale), int(844 * input_mask_scale)),
                    cv::Point(int(1023 * input_mask_scale), int(793 * input_mask_scale)),
                    cv::Point(int(1023 * input_mask_scale), int(1023 * input_mask_scale))};
                mask_lines.push_back(mask_line_0_0);
                mask_lines.push_back(mask_line_0_1);
                mask_lines.push_back(mask_line_1);
                mask_lines.push_back(mask_line_2_0);
                mask_lines.push_back(mask_line_2_1);
                mask_lines.push_back(mask_line_3);
                mask_lines.push_back(mask_line_5);
            }
            if (1 == input_concate_image_mask_car_name)
            {
                std::vector<cv::Point> mask_line_0_0{
                    cv::Point(int(0 * input_mask_scale), int(0 * input_mask_scale)),
                    cv::Point(int(10 * input_mask_scale), int(0 * input_mask_scale)),
                    cv::Point(int(25 * input_mask_scale), int(395 * input_mask_scale)),
                    cv::Point(int(138 * input_mask_scale), int(511 * input_mask_scale)),
                    cv::Point(int(0 * input_mask_scale), int(511 * input_mask_scale))};
                std::vector<cv::Point> mask_line_0_1{
                    cv::Point(int(280 * input_mask_scale), int(511 * input_mask_scale)),
                    cv::Point(int(300 * input_mask_scale), int(325 * input_mask_scale)),
                    cv::Point(int(315 * input_mask_scale), int(280 * input_mask_scale)),
                    cv::Point(int(341 * input_mask_scale), int(263 * input_mask_scale)),
                    cv::Point(int(341 * input_mask_scale), int(511 * input_mask_scale))};
                std::vector<cv::Point> mask_line_1{
                    cv::Point(int(342 * input_mask_scale), int(364 * input_mask_scale)),
                    cv::Point(int(412 * input_mask_scale), int(324 * input_mask_scale)),
                    cv::Point(int(581 * input_mask_scale), int(345 * input_mask_scale)),
                    cv::Point(int(634 * input_mask_scale), int(440 * input_mask_scale)),
                    cv::Point(int(681 * input_mask_scale), int(413 * input_mask_scale)),
                    cv::Point(int(681 * input_mask_scale), int(511 * input_mask_scale)),
                    cv::Point(int(342 * input_mask_scale), int(511 * input_mask_scale))};
                std::vector<cv::Point> mask_line_2_0{
                    cv::Point(int(682 * input_mask_scale), int(320 * input_mask_scale)),
                    cv::Point(int(701 * input_mask_scale), int(387 * input_mask_scale)),
                    cv::Point(int(716 * input_mask_scale), int(511 * input_mask_scale)),
                    cv::Point(int(682 * input_mask_scale), int(511 * input_mask_scale))};
                std::vector<cv::Point> mask_line_2_1{
                    cv::Point(int(893 * input_mask_scale), int(511 * input_mask_scale)),
                    cv::Point(int(1000 * input_mask_scale), int(392 * input_mask_scale)),
                    cv::Point(int(1008 * input_mask_scale), int(0 * input_mask_scale)),
                    cv::Point(int(1023 * input_mask_scale), int(0 * input_mask_scale)),
                    cv::Point(int(1023 * input_mask_scale), int(511 * input_mask_scale))};
                std::vector<cv::Point> mask_line_3{
                    cv::Point(int(0 * input_mask_scale), int(886 * input_mask_scale)),
                    cv::Point(int(26 * input_mask_scale), int(936 * input_mask_scale)),
                    cv::Point(int(30 * input_mask_scale), int(990 * input_mask_scale)),
                    cv::Point(int(100 * input_mask_scale), int(1023 * input_mask_scale)),
                    cv::Point(int(0 * input_mask_scale), int(1023 * input_mask_scale))};
                std::vector<cv::Point> mask_line_5{
                    cv::Point(int(902 * input_mask_scale), int(1023 * input_mask_scale)),
                    cv::Point(int(965 * input_mask_scale), int(960 * input_mask_scale)),
                    cv::Point(int(980 * input_mask_scale), int(873 * input_mask_scale)),
                    cv::Point(int(1023 * input_mask_scale), int(750 * input_mask_scale)),
                    cv::Point(int(1023 * input_mask_scale), int(1023 * input_mask_scale))};
                mask_lines.push_back(mask_line_0_0);
                mask_lines.push_back(mask_line_0_1);
                mask_lines.push_back(mask_line_1);
                mask_lines.push_back(mask_line_2_0);
                mask_lines.push_back(mask_line_2_1);
                mask_lines.push_back(mask_line_3);
                mask_lines.push_back(mask_line_5);
            }
        }

        cv::drawContours(concate_image_cpu, mask_lines, -1, cv::Scalar(0, 0, 0), -1);

        PrepareImage_mask_endTime_0 = clock();
        std::cout << "Mask self car time is:" << (double)(PrepareImage_mask_endTime_0 - PrepareImage_mask_startTime_0) / CLOCKS_PER_SEC << "s" << std::endl;

        // set input image
        // cv::Mat input_image;
        // input_image = cv::imread("/mnt/data_1/Dataset/dataset_sample/Autopilot_bev_dataset_sample/input/0401_2_000147.jpg");
        // cv::resize(input_image, image_resize, cv::Size(net_input_width_, net_input_height_));
        // cv::imshow("source image", image_resize);
        // cv::waitKey(0);

        // pub input net image
        if (show_result)
        {
            cv_bridge::CvImage input_net_cv_image;
            sensor_msgs::Image input_net_image;

            input_net_cv_image.image = concate_image_cpu;
            input_net_cv_image.encoding = "bgr8";
            input_net_cv_image.toImageMsg(input_net_image);
            pub_cross_view_input_net_image.publish(input_net_image);
        }

        PrepareImage_memcpy_startTime_0 = clock();

        cv::cuda::GpuMat gpu_image_resize;
        cv::cuda::GpuMat gpu_image_float;

        gpu_image_resize.upload(concate_image_cpu);
        std::vector<cv::cuda::GpuMat> bgrChannels_gpu(3);
        cv::cuda::split(gpu_image_resize, bgrChannels_gpu);
        for (auto i = 0; i < bgrChannels_gpu.size(); i++)
        {
            bgrChannels_gpu[i].convertTo(bgrChannels_gpu[i], CV_32FC1, 1.0 / (std[i] * 255.0), (0.0 - mean[i]) / std[i]);
        }
        cv::cuda::merge(bgrChannels_gpu, gpu_image_resize);
        gpu_image_resize.convertTo(gpu_image_float, CV_32FC3, 1.f);



        // std::vector<float> mean_value{0.45, 0.45, 0.45};
        // std::vector<float> std_value{0.225, 0.225, 0.225};
        // cv::Mat dst;
        // std::vector<cv::Mat> bgrChannels(3);
        // cv::split(concate_image_cpu, bgrChannels);
        // for (auto i = 0; i < bgrChannels.size(); i++)
        // {
        //     bgrChannels[i].convertTo(bgrChannels[i], CV_32FC1, 1.0 / (std_value[i]*255.0), (0.0 - mean_value[i]) / std_value[i]);
        //     // bgrChannels[i].convertTo(bgrChannels[i], CV_32FC1, 1.0/255.0);
        //     // bgrChannels[i]=(bgrChannels[i] - mean[i] ) * (1.0 / (std[i]));
        // }
        // cv::merge(bgrChannels, dst);
        // cv::subtract(concate_image_cpu,Scalar(0.45*255.0, 0.45*255.0, 0.45*255.0),concate_image_cpu);
        // cv::multiply(concate_image_cpu,Scalar(1/(0.225*255), 1/(0.225*255), 1/(0.225*255)),concate_image_cpu);



        PrepareImage_memcpy_endTime_0 = clock();

        PrepareImage_memcpy_startTime_0 = clock();

        // cv::cuda::GpuMat gpu_image_resize;
        // cv::cuda::GpuMat gpu_image_float;
        // gpu_image_resize.upload(dst);
        // gpu_image_resize.upload(concate_image_cpu);
        // gpu_image_resize.convertTo(gpu_image_float, CV_32FC3, 1.f / 255.f);
        // gpu_image_resize.convertTo(gpu_image_float, CV_32FC3, 1.0);


        std::vector<cv::cuda::GpuMat> chw;

        for (int i = 0; i < net_input_channel_; ++i)
            chw.emplace_back(cv::cuda::GpuMat(net_input_image_size,
                                              CV_32FC1, gpu_input + i * net_input_width_ * net_input_height_));
        cv::cuda::split(gpu_image_float, chw);

        PrepareImage_memcpy_endTime_0 = clock();
        std::cout << "Memcpy time is:" << (double)(PrepareImage_memcpy_endTime_0 - PrepareImage_memcpy_startTime_0) / CLOCKS_PER_SEC << "s" << std::endl;
    }

    PrepareImage_endTime_0 = clock();
    std::cout << "PrepareImage time is:" << (double)(PrepareImage_endTime_0 - PrepareImage_startTime_0) / CLOCKS_PER_SEC << "s" << std::endl;

    return;
}

std::vector<float> Camera_cross_view_base_opencv_gpu::PrepareImage_concate(std::vector<cv::Mat> image_vect)
{
    PrepareImage_startTime_0 = clock(); //计时开始

    auto net_input_image_size = cv::Size(net_input_width_, net_input_height_);

    std::vector<cv::cuda::GpuMat> image_vect_gpu;
    cv::cuda::GpuMat concate_image_gpu;
    int channelLength = net_input_image_size.area();

    std::vector<float> result(net_input_height_ * net_input_width_ * net_input_channel_ * net_input_batch_size_);
    auto data = result.data();
    cv::Mat concate_image(net_input_image_size,
                          CV_8UC3, cv::Scalar(0, 0, 0));
    // cv::cuda::GpuMat concate_image_gpu(net_input_image_size,
    //                                    CV_8UC3,
    //                                    cv::Scalar(0, 0, 0));

    if (1 == image_vect.size())
    {
        int channelLength = net_input_height_ * net_input_width_;

        cv::Mat image_resize;
        cv::Mat image_float;
        std::vector<cv::Mat> image_channels_data(net_input_channel_);

        // preprocessing
        PrepareImage_concate_resize_startTime_0 = clock();

        cv::resize(image_vect[0], image_resize, cv::Size(net_input_width_, net_input_height_));

        PrepareImage_concate_resize_endTime_0 = clock();
        std::cout << "Image resize and concate time is:" << (double)(PrepareImage_concate_resize_endTime_0 - PrepareImage_concate_resize_startTime_0) / CLOCKS_PER_SEC << "s" << std::endl;

        // mask self car
        PrepareImage_mask_startTime_0 = clock();

        std::vector<std::vector<cv::Point>> mask_lines;
        if (0 == input_concate_image_mask_car_name)
        {
            // front center
            std::vector<cv::Point> mask_line_0{
                cv::Point(int(0 * input_mask_scale), int(964 * input_mask_scale)),
                cv::Point(int(71 * input_mask_scale), int(943 * input_mask_scale)),
                cv::Point(int(171 * input_mask_scale), int(931 * input_mask_scale)),
                cv::Point(int(335 * input_mask_scale), int(939 * input_mask_scale)),
                cv::Point(int(453 * input_mask_scale), int(948 * input_mask_scale)),
                cv::Point(int(452 * input_mask_scale), int(850 * input_mask_scale)),
                cv::Point(int(469 * input_mask_scale), int(796 * input_mask_scale)),
                cv::Point(int(726 * input_mask_scale), int(789 * input_mask_scale)),
                cv::Point(int(772 * input_mask_scale), int(837 * input_mask_scale)),
                cv::Point(int(773 * input_mask_scale), int(934 * input_mask_scale)),
                cv::Point(int(1023 * input_mask_scale), int(897 * input_mask_scale)),
                cv::Point(int(1023 * input_mask_scale), int(1023 * input_mask_scale)),
                cv::Point(int(0 * input_mask_scale), int(1023 * input_mask_scale))};
            mask_lines.push_back(mask_line_0);
            // front right
            // std::vector<cv::Point> mask_line_0{
            //     cv::Point(int(0 * input_mask_scale), int(475 * input_mask_scale)),
            //     cv::Point(int(120 * input_mask_scale), int(680 * input_mask_scale)),
            //     cv::Point(int(130 * input_mask_scale), int(682 * input_mask_scale)),
            //     cv::Point(int(168 * input_mask_scale), int(713 * input_mask_scale)),
            //     cv::Point(int(280 * input_mask_scale), int(733 * input_mask_scale)),
            //     cv::Point(int(303 * input_mask_scale), int(744 * input_mask_scale)),
            //     cv::Point(int(672 * input_mask_scale), int(723 * input_mask_scale)),
            //     cv::Point(int(736 * input_mask_scale), int(750 * input_mask_scale)),
            //     cv::Point(int(885 * input_mask_scale), int(722 * input_mask_scale)),
            //     cv::Point(int(1023 * input_mask_scale), int(550 * input_mask_scale)),
            //     cv::Point(int(1023 * input_mask_scale), int(1023 * input_mask_scale)),
            //     cv::Point(int(0 * input_mask_scale), int(1023 * input_mask_scale))};
            // mask_lines.push_back(mask_line_0);
        }
        if (1 == input_concate_image_mask_car_name)
        {
            std::vector<cv::Point> mask_line_0{
                cv::Point(int(342 * input_mask_scale), int(364 * input_mask_scale)),
                cv::Point(int(412 * input_mask_scale), int(324 * input_mask_scale)),
                cv::Point(int(581 * input_mask_scale), int(345 * input_mask_scale)),
                cv::Point(int(634 * input_mask_scale), int(440 * input_mask_scale)),
                cv::Point(int(681 * input_mask_scale), int(413 * input_mask_scale)),
                cv::Point(int(681 * input_mask_scale), int(511 * input_mask_scale)),
                cv::Point(int(342 * input_mask_scale), int(511 * input_mask_scale))};
            mask_lines.push_back(mask_line_0);
        }

        cv::drawContours(image_resize, mask_lines, -1, cv::Scalar(0, 0, 0), -1);

        PrepareImage_mask_endTime_0 = clock();
        std::cout << "Mask self car time is:" << (double)(PrepareImage_mask_endTime_0 - PrepareImage_mask_startTime_0) / CLOCKS_PER_SEC << "s" << std::endl;

        // set input image
        // cv::Mat input_image;
        // input_image = cv::imread("/mnt/data_1/Dataset/dataset_sample/Autopilot_bev_dataset_sample/input/0401_2_000147.jpg");
        // cv::resize(input_image, image_resize, cv::Size(net_input_width_, net_input_height_));
        // cv::imshow("source image", image_resize);
        // cv::waitKey(0);

        // pub input net image
        if (show_result)
        {
            cv_bridge::CvImage input_net_cv_image;
            sensor_msgs::Image input_net_image;

            input_net_cv_image.image = image_resize;
            input_net_cv_image.encoding = "bgr8";
            input_net_cv_image.toImageMsg(input_net_image);
            pub_cross_view_input_net_image.publish(input_net_image);
        }

        PrepareImage_memcpy_startTime_0 = clock();

        image_resize.convertTo(image_float, CV_32FC3, 1);
        cv::split(image_float, image_channels_data);
        for (size_t j = 0; j < net_input_channel_; j++)
        {
            cv::Mat normed_channel;
            normed_channel = (image_channels_data[j] - mean[j] * 255) * (1 / (std[j] * 255));
            memcpy(data, normed_channel.data, channelLength * sizeof(float));
            data += channelLength;
        }

        PrepareImage_memcpy_endTime_0 = clock();
        std::cout << "Memcpy time is:" << (double)(PrepareImage_memcpy_endTime_0 - PrepareImage_memcpy_startTime_0) / CLOCKS_PER_SEC << "s" << std::endl;
    }
    if (3 == image_vect.size())
    {
        int channelLength = net_input_height_ * net_input_width_;

        // concate image
        std::vector<cv::Mat> concate_image_row_0;
        cv::Mat concate_image_row_result_0;
        std::vector<cv::Mat> concate_image_row_1;
        cv::Mat concate_image_row_result_1;
        std::vector<cv::Mat> concate_image_row;
        cv::Mat concate_image;

        PrepareImage_concate_resize_startTime_0 = clock();

        // concate
        concate_image_row_0.push_back(image_vect[1]);
        concate_image_row_0.push_back(image_vect[0]);
        concate_image_row_0.push_back(image_vect[2]);
        cv::hconcat(concate_image_row_0, concate_image);

        cv::Mat image_resize;
        cv::Mat image_float;
        std::vector<cv::Mat> image_channels_data(net_input_channel_);

        // reszie
        cv::resize(concate_image, image_resize, cv::Size(net_input_width_, net_input_height_));

        PrepareImage_concate_resize_endTime_0 = clock();
        std::cout << "Image resize and concate time is:" << (double)(PrepareImage_concate_resize_endTime_0 - PrepareImage_concate_resize_startTime_0) / CLOCKS_PER_SEC << "s" << std::endl;

        // mask self car
        PrepareImage_mask_startTime_0 = clock();

        std::vector<std::vector<cv::Point>> mask_lines;
        std::vector<cv::Point> mask_line_0{
            cv::Point(0, 0),
            cv::Point(10, 0),
            cv::Point(25, 790),
            cv::Point(161, 1024),
            cv::Point(280, 1024),
            cv::Point(300, 700),
            cv::Point(315, 560),
            cv::Point(334, 536),
            cv::Point(341, 540),
            cv::Point(341, 1024),
            cv::Point(0, 1024),
            cv::Point(0, 0)};
        std::vector<cv::Point> mask_line_1{
            cv::Point(341, 728),
            cv::Point(424, 650),
            cv::Point(555, 688),
            cv::Point(570, 706),
            cv::Point(645, 912),
            cv::Point(682, 862),
            cv::Point(682, 1024),
            cv::Point(341, 1024)};
        std::vector<cv::Point> mask_line_2{
            cv::Point(682, 646),
            cv::Point(716, 1024),
            cv::Point(865, 1024),
            cv::Point(995, 788),
            cv::Point(1007, 0),
            cv::Point(1024, 0),
            cv::Point(1024, 1024),
            cv::Point(682, 1024)};
        mask_lines.push_back(mask_line_0);
        mask_lines.push_back(mask_line_1);
        mask_lines.push_back(mask_line_2);
        cv::drawContours(image_resize, mask_lines, -1, cv::Scalar(0, 0, 0), -1);

        PrepareImage_mask_endTime_0 = clock();
        std::cout << "Mask self car time is:" << (double)(PrepareImage_mask_endTime_0 - PrepareImage_mask_startTime_0) / CLOCKS_PER_SEC << "s" << std::endl;

        // set input image
        // cv::Mat input_image;
        // input_image = cv::imread("/mnt/data_1/Dataset/dataset_sample/Autopilot_bev_dataset_sample/input/0401_2_000147.jpg");
        // cv::resize(input_image, image_resize, cv::Size(net_input_width_, net_input_height_));
        // cv::imshow("source image", image_resize);
        // cv::waitKey(0);

        // pub input net image
        if (show_result)
        {
            cv_bridge::CvImage input_net_cv_image;
            sensor_msgs::Image input_net_image;

            input_net_cv_image.image = image_resize;
            input_net_cv_image.encoding = "bgr8";
            input_net_cv_image.toImageMsg(input_net_image);
            pub_cross_view_input_net_image.publish(input_net_image);
        }

        PrepareImage_memcpy_startTime_0 = clock();

        image_resize.convertTo(image_float, CV_32FC3, 1);
        cv::split(image_float, image_channels_data);
        for (size_t j = 0; j < net_input_channel_; j++)
        {
            cv::Mat normed_channel;
            normed_channel = (image_channels_data[j] - mean[j] * 255) * (1 / (std[j] * 255));
            memcpy(data, normed_channel.data, channelLength * sizeof(float));
            data += channelLength;
        }

        PrepareImage_memcpy_endTime_0 = clock();
        std::cout << "Memcpy time is:" << (double)(PrepareImage_memcpy_endTime_0 - PrepareImage_memcpy_startTime_0) / CLOCKS_PER_SEC << "s" << std::endl;
    }
    if (6 == image_vect.size())
    {
        for (size_t i = 0; i < image_vect.size(); i++)
        {
            cv::cuda::GpuMat image_gpu;
            cv::cuda::GpuMat image_resize_gpu;
            cv::Mat cpu_image_resize;

            auto gpu_image_size = cv::Size(int(net_input_width_ / 3), int(net_input_height_ / 2));

            cv::Mat cpu_image = image_vect[i];
            image_gpu.upload(cpu_image);

            cv::cuda::resize(image_gpu, image_resize_gpu, gpu_image_size);
            image_resize_gpu.download(cpu_image_resize);
            image_vect[i] = cpu_image_resize;
        }

        PrepareImage_concate_resize_startTime_0 = clock();

        // concate
        std::vector<cv::Mat> concate_image_row_0;
        cv::Mat concate_image_row_result_0;
        std::vector<cv::Mat> concate_image_row_1;
        cv::Mat concate_image_row_result_1;
        std::vector<cv::Mat> concate_image_row;
        cv::Mat concate_image;

        concate_image_row_0.push_back(image_vect[1]);
        concate_image_row_0.push_back(image_vect[0]);
        concate_image_row_0.push_back(image_vect[3]);
        cv::hconcat(concate_image_row_0, concate_image_row_result_0);

        concate_image_row_1.push_back(image_vect[2]);
        concate_image_row_1.push_back(image_vect[5]);
        concate_image_row_1.push_back(image_vect[4]);
        cv::hconcat(concate_image_row_1, concate_image_row_result_1);

        concate_image_row.push_back(concate_image_row_result_0);
        concate_image_row.push_back(concate_image_row_result_1);
        cv::vconcat(concate_image_row, concate_image);

        cv::Mat image_resize;
        cv::Mat image_float;
        std::vector<cv::Mat> image_channels_data(net_input_channel_);

        // resize
        cv::resize(concate_image, image_resize, cv::Size(net_input_width_, net_input_height_));

        PrepareImage_concate_resize_endTime_0 = clock();
        std::cout << "Image resize and concate time is:" << (double)(PrepareImage_concate_resize_endTime_0 - PrepareImage_concate_resize_startTime_0) / CLOCKS_PER_SEC << "s" << std::endl;

        // mask self car
        PrepareImage_mask_startTime_0 = clock();

        std::vector<std::vector<cv::Point>> mask_lines;
        if (0 == input_concate_image_mask_car_name)
        {
            std::vector<cv::Point> mask_line_0_0{
                cv::Point(int(0 * input_mask_scale), int(0 * input_mask_scale)),
                cv::Point(int(8 * input_mask_scale), int(0 * input_mask_scale)),
                cv::Point(int(27 * input_mask_scale), int(427 * input_mask_scale)),
                cv::Point(int(98 * input_mask_scale), int(511 * input_mask_scale)),
                cv::Point(int(0 * input_mask_scale), int(511 * input_mask_scale)),
                cv::Point(int(0 * input_mask_scale), int(0 * input_mask_scale))};
            std::vector<cv::Point> mask_line_0_1{
                cv::Point(int(273 * input_mask_scale), int(511 * input_mask_scale)),
                cv::Point(int(290 * input_mask_scale), int(376 * input_mask_scale)),
                cv::Point(int(308 * input_mask_scale), int(292 * input_mask_scale)),
                cv::Point(int(321 * input_mask_scale), int(287 * input_mask_scale)),
                cv::Point(int(327 * input_mask_scale), int(280 * input_mask_scale)),
                cv::Point(int(336 * input_mask_scale), int(280 * input_mask_scale)),
                cv::Point(int(341 * input_mask_scale), int(281 * input_mask_scale)),
                cv::Point(int(341 * input_mask_scale), int(511 * input_mask_scale))};
            // front center
            std::vector<cv::Point> mask_line_1{
                cv::Point(int(341 * input_mask_scale), int(482 * input_mask_scale)),
                cv::Point(int(360 * input_mask_scale), int(473 * input_mask_scale)),
                cv::Point(int(395 * input_mask_scale), int(466 * input_mask_scale)),
                cv::Point(int(446 * input_mask_scale), int(469 * input_mask_scale)),
                cv::Point(int(492 * input_mask_scale), int(474 * input_mask_scale)),
                cv::Point(int(492 * input_mask_scale), int(424 * input_mask_scale)),
                cv::Point(int(497 * input_mask_scale), int(398 * input_mask_scale)),
                cv::Point(int(583 * input_mask_scale), int(395 * input_mask_scale)),
                cv::Point(int(598 * input_mask_scale), int(419 * input_mask_scale)),
                cv::Point(int(599 * input_mask_scale), int(467 * input_mask_scale)),
                cv::Point(int(681 * input_mask_scale), int(449 * input_mask_scale)),
                cv::Point(int(681 * input_mask_scale), int(511 * input_mask_scale)),
                cv::Point(int(341 * input_mask_scale), int(511 * input_mask_scale))};
            // front right
            // std::vector<cv::Point> mask_line_1{
            //     cv::Point(int(342 * input_mask_scale), int(237 * input_mask_scale)),
            //     cv::Point(int(385 * input_mask_scale), int(340 * input_mask_scale)),
            //     cv::Point(int(398 * input_mask_scale), int(356 * input_mask_scale)),
            //     cv::Point(int(433 * input_mask_scale), int(365 * input_mask_scale)),
            //     cv::Point(int(433 * input_mask_scale), int(371 * input_mask_scale)),
            //     cv::Point(int(565 * input_mask_scale), int(360 * input_mask_scale)),
            //     cv::Point(int(587 * input_mask_scale), int(374 * input_mask_scale)),
            //     cv::Point(int(636 * input_mask_scale), int(358 * input_mask_scale)),
            //     cv::Point(int(681 * input_mask_scale), int(274 * input_mask_scale)),
            //     cv::Point(int(681 * input_mask_scale), int(511 * input_mask_scale)),
            //     cv::Point(int(342 * input_mask_scale), int(511 * input_mask_scale))};
            std::vector<cv::Point> mask_line_2_0{
                cv::Point(int(682 * input_mask_scale), int(293 * input_mask_scale)),
                cv::Point(int(689 * input_mask_scale), int(305 * input_mask_scale)),
                cv::Point(int(702 * input_mask_scale), int(310 * input_mask_scale)),
                cv::Point(int(730 * input_mask_scale), int(511 * input_mask_scale)),
                cv::Point(int(682 * input_mask_scale), int(511 * input_mask_scale))};
            std::vector<cv::Point> mask_line_2_1{
                cv::Point(int(920 * input_mask_scale), int(511 * input_mask_scale)),
                cv::Point(int(967 * input_mask_scale), int(471 * input_mask_scale)),
                cv::Point(int(1012 * input_mask_scale), int(416 * input_mask_scale)),
                cv::Point(int(1023 * input_mask_scale), int(295 * input_mask_scale)),
                cv::Point(int(1023 * input_mask_scale), int(511 * input_mask_scale))};
            std::vector<cv::Point> mask_line_3{
                cv::Point(int(0 * input_mask_scale), int(877 * input_mask_scale)),
                cv::Point(int(8 * input_mask_scale), int(900 * input_mask_scale)),
                cv::Point(int(16 * input_mask_scale), int(910 * input_mask_scale)),
                cv::Point(int(28 * input_mask_scale), int(943 * input_mask_scale)),
                cv::Point(int(35 * input_mask_scale), int(981 * input_mask_scale)),
                cv::Point(int(96 * input_mask_scale), int(1023 * input_mask_scale)),
                cv::Point(int(0 * input_mask_scale), int(1023 * input_mask_scale))};
            std::vector<cv::Point> mask_line_5{
                cv::Point(int(912 * input_mask_scale), int(1023 * input_mask_scale)),
                cv::Point(int(932 * input_mask_scale), int(999 * input_mask_scale)),
                cv::Point(int(975 * input_mask_scale), int(975 * input_mask_scale)),
                cv::Point(int(985 * input_mask_scale), int(903 * input_mask_scale)),
                cv::Point(int(997 * input_mask_scale), int(858 * input_mask_scale)),
                cv::Point(int(1006 * input_mask_scale), int(844 * input_mask_scale)),
                cv::Point(int(1023 * input_mask_scale), int(793 * input_mask_scale)),
                cv::Point(int(1023 * input_mask_scale), int(1023 * input_mask_scale))};
            mask_lines.push_back(mask_line_0_0);
            mask_lines.push_back(mask_line_0_1);
            mask_lines.push_back(mask_line_1);
            mask_lines.push_back(mask_line_2_0);
            mask_lines.push_back(mask_line_2_1);
            mask_lines.push_back(mask_line_3);
            mask_lines.push_back(mask_line_5);
        }
        if (1 == input_concate_image_mask_car_name)
        {
            std::vector<cv::Point> mask_line_0_0{
                cv::Point(int(0 * input_mask_scale), int(0 * input_mask_scale)),
                cv::Point(int(10 * input_mask_scale), int(0 * input_mask_scale)),
                cv::Point(int(25 * input_mask_scale), int(395 * input_mask_scale)),
                cv::Point(int(138 * input_mask_scale), int(511 * input_mask_scale)),
                cv::Point(int(0 * input_mask_scale), int(511 * input_mask_scale))};
            std::vector<cv::Point> mask_line_0_1{
                cv::Point(int(280 * input_mask_scale), int(511 * input_mask_scale)),
                cv::Point(int(300 * input_mask_scale), int(325 * input_mask_scale)),
                cv::Point(int(315 * input_mask_scale), int(280 * input_mask_scale)),
                cv::Point(int(341 * input_mask_scale), int(263 * input_mask_scale)),
                cv::Point(int(341 * input_mask_scale), int(511 * input_mask_scale))};
            std::vector<cv::Point> mask_line_1{
                cv::Point(int(342 * input_mask_scale), int(364 * input_mask_scale)),
                cv::Point(int(412 * input_mask_scale), int(324 * input_mask_scale)),
                cv::Point(int(581 * input_mask_scale), int(345 * input_mask_scale)),
                cv::Point(int(634 * input_mask_scale), int(440 * input_mask_scale)),
                cv::Point(int(681 * input_mask_scale), int(413 * input_mask_scale)),
                cv::Point(int(681 * input_mask_scale), int(511 * input_mask_scale)),
                cv::Point(int(342 * input_mask_scale), int(511 * input_mask_scale))};
            std::vector<cv::Point> mask_line_2_0{
                cv::Point(int(682 * input_mask_scale), int(320 * input_mask_scale)),
                cv::Point(int(701 * input_mask_scale), int(387 * input_mask_scale)),
                cv::Point(int(716 * input_mask_scale), int(511 * input_mask_scale)),
                cv::Point(int(682 * input_mask_scale), int(511 * input_mask_scale))};
            std::vector<cv::Point> mask_line_2_1{
                cv::Point(int(893 * input_mask_scale), int(511 * input_mask_scale)),
                cv::Point(int(1000 * input_mask_scale), int(392 * input_mask_scale)),
                cv::Point(int(1008 * input_mask_scale), int(0 * input_mask_scale)),
                cv::Point(int(1023 * input_mask_scale), int(0 * input_mask_scale)),
                cv::Point(int(1023 * input_mask_scale), int(511 * input_mask_scale))};
            std::vector<cv::Point> mask_line_3{
                cv::Point(int(0 * input_mask_scale), int(886 * input_mask_scale)),
                cv::Point(int(26 * input_mask_scale), int(936 * input_mask_scale)),
                cv::Point(int(30 * input_mask_scale), int(990 * input_mask_scale)),
                cv::Point(int(100 * input_mask_scale), int(1023 * input_mask_scale)),
                cv::Point(int(0 * input_mask_scale), int(1023 * input_mask_scale))};
            std::vector<cv::Point> mask_line_5{
                cv::Point(int(902 * input_mask_scale), int(1023 * input_mask_scale)),
                cv::Point(int(965 * input_mask_scale), int(960 * input_mask_scale)),
                cv::Point(int(980 * input_mask_scale), int(873 * input_mask_scale)),
                cv::Point(int(1023 * input_mask_scale), int(750 * input_mask_scale)),
                cv::Point(int(1023 * input_mask_scale), int(1023 * input_mask_scale))};
            mask_lines.push_back(mask_line_0_0);
            mask_lines.push_back(mask_line_0_1);
            mask_lines.push_back(mask_line_1);
            mask_lines.push_back(mask_line_2_0);
            mask_lines.push_back(mask_line_2_1);
            mask_lines.push_back(mask_line_3);
            mask_lines.push_back(mask_line_5);
        }

        cv::drawContours(image_resize, mask_lines, -1, cv::Scalar(0, 0, 0), -1);

        PrepareImage_mask_endTime_0 = clock();
        std::cout << "Mask self car time is:" << (double)(PrepareImage_mask_endTime_0 - PrepareImage_mask_startTime_0) / CLOCKS_PER_SEC << "s" << std::endl;

        // pub input net image
        if (show_result)
        {
            cv_bridge::CvImage input_net_cv_image;
            sensor_msgs::Image input_net_image;

            input_net_cv_image.image = image_resize;
            input_net_cv_image.encoding = "bgr8";
            input_net_cv_image.toImageMsg(input_net_image);
            pub_cross_view_input_net_image.publish(input_net_image);
        }

        PrepareImage_memcpy_startTime_0 = clock();

        image_resize.convertTo(image_float, CV_32FC3, 1);
        cv::split(image_float, image_channels_data);
        for (size_t j = 0; j < net_input_channel_; j++)
        {
            cv::Mat normed_channel;
            normed_channel = (image_channels_data[j] - mean[j] * 255) * (1 / (std[j] * 255));
            memcpy(data, normed_channel.data, channelLength * sizeof(float));
            data += channelLength;
        }

        PrepareImage_memcpy_endTime_0 = clock();
        std::cout << "Memcpy time is:" << (double)(PrepareImage_memcpy_endTime_0 - PrepareImage_memcpy_startTime_0) / CLOCKS_PER_SEC << "s" << std::endl;
    }

    PrepareImage_endTime_0 = clock();
    std::cout << "PrepareImage time is:" << (double)(PrepareImage_endTime_0 - PrepareImage_startTime_0) / CLOCKS_PER_SEC << "s" << std::endl;

    return result;
}

double Camera_cross_view_base_opencv_gpu::get_current_frame_image_timestamp(const sensor_msgs::ImageConstPtr &image_message_0)
{
    double current_frame_timestamp;
    std::stringstream current_frame_timestamp_stringstream;

    current_frame_timestamp_stringstream << image_message_0->header.stamp.sec << "." << image_message_0->header.stamp.nsec;
    current_frame_timestamp_stringstream >> current_frame_timestamp;

    return current_frame_timestamp;
}

void Camera_cross_view_base_opencv_gpu::drawCrossMark(cv::Mat dst, cv::Point pt, int model_type)
{
    const int cross_len = 30;
    CvPoint pt1, pt2, pt3, pt4;

    pt1.x = pt.x;
    pt1.y = pt.y - cross_len;
    pt2.x = pt.x;
    pt2.y = pt.y + cross_len;
    pt3.x = pt.x - cross_len;
    pt3.y = pt.y;
    pt4.x = pt.x + cross_len;
    pt4.y = pt.y;

    if (0 == model_type)
    {
        cv::line(dst, pt1, pt2, cv::Scalar(255), 2, 16, 0);
        cv::line(dst, pt3, pt4, cv::Scalar(255), 2, 16, 0);
    }
    else
    {
        cv::line(dst, pt1, pt2, cv::Scalar(0), 2, 16, 0);
        cv::line(dst, pt3, pt4, cv::Scalar(0), 2, 16, 0);
    }

    return;
}
